#include "corners.h"
#include "utils.h"
#include <queue>

void hallarPoligonos(Mat img,vector<Vec4i> lines,vector<vector<Point2f> > &corners){
	int* poly = new int[lines.size()];  
	for(size_t i=0;i<lines.size();i++)poly[i] = - 1;  
	int curPoly = 0;  
	for (size_t i = 0; i < lines.size(); i++)  
	{  
		for (size_t j = i+1; j < lines.size(); j++)  
		{  

			Point2f pt = computeIntersect(lines[i], lines[j]);  
			if (pt.x >= 0 && pt.y >= 0 && pt.x<img.size().width && pt.y<img.size().height){  

				if(poly[i]==-1&&poly[j] == -1){  
					vector<Point2f> v;  
					v.push_back(pt);  
					corners.push_back(v);       
					poly[i] = curPoly;  
					poly[j] = curPoly;  
					curPoly++;  
					continue;  
				}  
				if(poly[i]==-1&&poly[j]>=0){  
					corners[poly[j]].push_back(pt);  
					poly[i] = poly[j];  
					continue;  
				}  
				if(poly[i]>=0&&poly[j]==-1){  
					corners[poly[i]].push_back(pt);  
					poly[j] = poly[i];  
					continue;  
				}  
				if(poly[i]>=0&&poly[j]>=0){  
					if(poly[i]==poly[j]){  
						corners[poly[i]].push_back(pt);  
						continue;  
					}  
					for(size_t k=0;k<corners[poly[j]].size();k++){  
						corners[poly[i]].push_back(corners[poly[j]][k]);  
					}  
					corners[poly[j]].clear();  
					poly[j] = poly[i];  
					continue;  
				}  
			}  
		}  
	} 
}
int distanciaMinimaLinea = 40;
bool esCuadrilatero(vector<Point2f> grafo,int p1,int p2,int p3,int p4){
	if (distanceBetween(grafo[p1],grafo[p2]) < distanciaMinimaLinea) return false;
	if (distanceBetween(grafo[p2],grafo[p3]) < distanciaMinimaLinea) return false;
	if (distanceBetween(grafo[p3],grafo[p4]) < distanciaMinimaLinea) return false;
	if (distanceBetween(grafo[p4],grafo[p1]) < distanciaMinimaLinea) return false;
	return true;
}

void hallarCircuitos4Nodos(int* matrizAdyacencia,int nNodos,vector<Vec4i> &circuitos,vector<Point2f> grafo){
	int *explorado = (int*)malloc(nNodos*sizeof(int));
	for (int i = 0; i < nNodos - 1; i++){
		queue<int> cola;
		vector<vector<int> > padres;
		padres.resize(nNodos);
		for (int j = 0; j < nNodos; ++j)
			explorado[j] = 0;
		cola.push(i);
		explorado[i] = 1; 
		while (!cola.empty()) {
			int v = cola.front();
			cola.pop();
			if (v!=i &&(matrizAdyacencia[i*nNodos + v] == 0))
				break;
			for (int w = 0; w < nNodos; ++w){
				if ((w > i) && matrizAdyacencia[v*nNodos + w]) {
					if (!explorado[w]) {
						cola.push(w);
						explorado[w] = true;
					}
					if (!matrizAdyacencia[w*nNodos + i])
						padres[w].push_back(v);
				}
			}
		}
		// Para todos los nodos procesados que no sean la raiz o uno de sus hijos se evaluan sus padres
		for (int j = 0; j < nNodos; j++){
			if (j==i || padres[j].size() < 2 || padres[j][0] == i)
				continue;
			int nPadres = padres[j].size();
			for (int m = 0; m < nPadres - 1; m++){
				for (int n = m+1; n < nPadres; n++){
					if (!matrizAdyacencia[padres[j][m]*nNodos + padres[j][n]]){ // Si los dos nodos no estan en la misma linea se agrega el circuito
						if (esCuadrilatero(grafo,i,padres[j][m],j,padres[j][n])){
							Vec4i circuito (i,padres[j][m],j,padres[j][n]);
							circuitos.push_back(circuito);
						}
					}
				}
			}
		}
	}
	free(explorado);
}

void hallarPoligonosGrafo(Mat img,vector<Vec4i> lines,vector<vector<Point2f> > &corners){
	vector<Point2f> grafo;
	vector<vector<int> > puntosEnLinea;
	puntosEnLinea.resize(lines.size());
	int iNodo = 0;
	for (size_t i = 0; i < lines.size(); i++)  
	{  
		for (size_t j = i+1; j < lines.size(); j++)  
		{  
			Point2f pt = computeIntersect(lines[i], lines[j]);  
			if (pt.x >= 0 && pt.y >= 0 && pt.x<img.size().width && pt.y<img.size().height){  
				grafo.push_back(pt);
				puntosEnLinea[i].push_back(iNodo);
				puntosEnLinea[j].push_back(iNodo);
				iNodo++;
			}
		}
	}
	int nNodos = grafo.size();
	int * matrizAdyacencia = (int *)malloc(nNodos*nNodos*sizeof(int));
	//inicializar la matriz de adyacencia
	for (int i = 0; i < nNodos*nNodos; i++)
		matrizAdyacencia[i] = 0;
	for (size_t i = 0; i < lines.size(); i++)  
	{  
		vector<int> nodos = puntosEnLinea[i];
		int nNodosEnLinea = nodos.size();
		if (nNodosEnLinea < 2) continue;
		for (int j = 0; j < nNodosEnLinea - 1;j++){
			for (int k = j+1; k < nNodosEnLinea;k++){
				if (distanceBetween(grafo[nodos[j]],grafo[nodos[k]]) > distanciaMinimaLinea){
					matrizAdyacencia[nodos[j]*nNodos + nodos[k]] = 1;
					matrizAdyacencia[nodos[k]*nNodos + nodos[j]] = 1;
				}
			}
		}
	}
	vector<Vec4i> circuitos;
	hallarCircuitos4Nodos(matrizAdyacencia,nNodos,circuitos,grafo);
	for (int i = 0; i < circuitos.size(); i++){
		Vec4i cuadrilatero = circuitos[i];
		vector<Point2f> corner;
		corner.push_back(grafo[cuadrilatero[0]]);
		corner.push_back(grafo[cuadrilatero[1]]);
		corner.push_back(grafo[cuadrilatero[2]]);
		corner.push_back(grafo[cuadrilatero[3]]);
		corners.push_back(corner);
	}
	free(matrizAdyacencia);

}